#include "stdafx.h"
#include "CoordConvert.h"
#include "LatLongZone.h"

const double PI = 3.14159265358979323846;

//-+---------------------------------------------------------------------------+-
//						Longitude and Latitude to UTM 
//-+---------------------------------------------------------------------------+-

// equatorial radius
double LonLat2UTM::equatorialRadius = 6378137;

// polar radius
double LonLat2UTM::polarRadius = 6356752.314;

// flattening
double LonLat2UTM::flattening = 0.00335281066474748;// (equatorialRadius-polarRadius)/equatorialRadius;

// inverse flattening 1/flattening
double LonLat2UTM::inverseFlattening = 298.257223563;// 1/flattening;

// Mean radius
double LonLat2UTM::rm = pow(equatorialRadius * polarRadius, 1 / 2.0);

// scale factor
double LonLat2UTM::k0 = 0.9996;

// eccentricity
double LonLat2UTM::e = sqrt(1 - pow(polarRadius / equatorialRadius, 2));

double LonLat2UTM::e1sq = e * e / (1 - e * e);

double LonLat2UTM::n = (equatorialRadius - polarRadius) / (equatorialRadius + polarRadius);

// r curv 1
double LonLat2UTM::rho = 6368573.744;

// r curv 2
double LonLat2UTM::nu = 6389236.914;

// Calculate Meridional Arc Length
// Meridional Arc
double LonLat2UTM::S = 5103266.421;

double LonLat2UTM::A0 = 6367449.146;

double LonLat2UTM::B0 = 16038.42955;

double LonLat2UTM::C0 = 16.83261333;

double LonLat2UTM::D0 = 0.021984404;

double LonLat2UTM::E0 = 0.000312705;

// Calculation ConsMath.Tants
// Delta Long
double LonLat2UTM::p = -0.483084;

double LonLat2UTM::sin1 = 4.84814E-06;

// Coefficients for UTM Coordinates
double LonLat2UTM::K1 = 5101225.115;

double LonLat2UTM::K2 = 3750.291596;

double LonLat2UTM::K3 = 1.397608151;

double LonLat2UTM::K4 = 214839.3105;

double LonLat2UTM::K5 = -2.995382942;

double LonLat2UTM::A6 = -1.00541E-07;

void LonLat2UTM::convertLonLatToUTM(double longitude, double latitude,  double* easting, double* northing, wchar_t* szUTM)
{
	// set variables
	SetVariable(latitude, longitude);

	// get zone CString
	wchar_t szlongZone[MAX_LENGTH], szlatZone;
	getLongZone(longitude, szlongZone);
	szlatZone  = getLatZone(latitude);

	*easting  = getEasting();
	*northing = getNorthing(latitude);

	wchar_t szEast[MAX_LENGTH], szNorth[MAX_LENGTH];
	_itot((int)*easting, szEast, 10);
	_itot((int)*northing,szNorth,10);

	size_t curlen = 0;
	memset(szUTM, 0, MAX_LENGTH);		// set buffer using 0
	
	_tcscat(szUTM, szlongZone);  // fill longzone CString

	curlen = wcslen(szUTM);		// fill whitespace
	szUTM[curlen] = _T(' ');

	szUTM[curlen+1]= szlatZone;	// fill latzone CString

	curlen = wcslen(szUTM);		// fill whitespace
	szUTM[curlen] = _T(' ');

	_tcscat(szUTM, szEast);		// fill esat data
	
	curlen = wcslen(szUTM);		// fill whitespace
	szUTM[curlen] = _T(' ');

	_tcscat(szUTM, szNorth);		// fill west data
}

double LonLat2UTM::getNorthing(double latitude)
{
	double northing = K1 + K2 * p * p + K3 * pow(p, 4);
	if (latitude < 0.0)
	{
		northing = 10000000 + northing;
	}
	return northing;
}

double LonLat2UTM::getEasting()
{
	return 500000 + (K4 * p + K5 * pow(p, 3));
}

void LonLat2UTM::SetVariable(double latitude, double longitude)
{
	latitude = latitude * PI / 180;
	rho = equatorialRadius * (1 - e * e) / pow(1 - pow(e * sin(latitude), 2), 3 / 2.0);

	nu = equatorialRadius / pow(1 - pow(e * sin(latitude), 2), (1 / 2.0));

	double var1;
	if (longitude < 0.0)
	{
		var1 = ((int)((180 + longitude) / 6.0)) + 1;
	}
	else
	{
		var1 = ((int)(longitude / 6)) + 31;
	}
	double var2 = (6 * var1) - 183;
	double var3 = longitude - var2;
	p = var3 * 3600 / 10000;

	S = A0 * latitude - B0 * sin(2 * latitude) + C0 * sin(4 * latitude) - D0 * sin(6 * latitude) + E0 * sin(8 * latitude);

	K1 = S * k0;
	K2 = nu * sin(latitude) * cos(latitude) * pow(sin1, 2) * k0 * (100000000)
		/ 2;
	K3 = ((pow(sin1, 4) * nu * sin(latitude) * pow(cos(latitude), 3)) / 24)
		* (5 - pow(tan(latitude), 2) + 9 * e1sq * pow(cos(latitude), 2) + 4
		* pow(e1sq, 2) * pow(cos(latitude), 4))
		* k0
		* (10000000000000000L);

	K4 = nu * cos(latitude) * sin1 * k0 * 10000;

	K5 = pow(sin1 * cos(latitude), 3) * (nu / 6)
		* (1 - pow(tan(latitude), 2) + e1sq * pow(cos(latitude), 2)) * k0
		* 1000000000000L;

	A6 = (pow(p * sin1, 6) * nu * sin(latitude) * pow(cos(latitude), 5) / 720)
		* (61 - 58 * pow(tan(latitude), 2) + pow(tan(latitude), 4) + 270
		* e1sq * pow(cos(latitude), 2) - 330 * e1sq
		* pow(sin(latitude), 2)) * k0 * (1E+24);
}


//-+---------------------------------------------------------------------------+-
//						UTM to Longitude and Latitude
//-+---------------------------------------------------------------------------+-
double UTM2LonLat::easting = 0.0;

double UTM2LonLat::northing = 0.0;

int UTM2LonLat::zone = 0;

double UTM2LonLat::arc = 0.0;

double UTM2LonLat::mu = 0.0;

double UTM2LonLat::ei = 0.0;

double UTM2LonLat::ca = 0.0;

double UTM2LonLat::cb = 0.0;

double UTM2LonLat::cc = 0.0;

double UTM2LonLat::cd = 0.0;

double UTM2LonLat::n0 = 0.0;

double UTM2LonLat::r0 = 0.0;

double UTM2LonLat::_a1 = 0.0;

double UTM2LonLat::dd0 = 0.0;

double UTM2LonLat::t0 = 0.0;

double UTM2LonLat::Q0 = 0.0;

double UTM2LonLat::lof1 = 0.0;

double UTM2LonLat::lof2 = 0.0;

double UTM2LonLat::lof3 = 0.0;

double UTM2LonLat::_a2 = 0.0;

double UTM2LonLat::phi1 = 0.0;

double UTM2LonLat::fact1 = 0.0;

double UTM2LonLat::fact2 = 0.0;

double UTM2LonLat::fact3 = 0.0;

double UTM2LonLat::fact4 = 0.0;

double UTM2LonLat::zoneCM = 0.0;

double UTM2LonLat::_a3 = 0.0;

wchar_t UTM2LonLat::southernHemisphere[11] = {'A','C','D','E','F','G','H','J','K','L','M'};

double UTM2LonLat::b = 6356752.314;

double UTM2LonLat::a = 6378137;

double UTM2LonLat::e = 0.081819191;

double UTM2LonLat::e1sq = 0.006739497;

double UTM2LonLat::k0 = 0.9996;


void UTM2LonLat::convertUTMToLongLat(wchar_t* pszUTM, double* latitude, double* longitude)
{
	CString strUTM(pszUTM);
	if (strUTM.GetLength() == 0) return;

	int iBlank = strUTM.Find(_T(' '));
	zone = _ttoi(strUTM.Mid(0, iBlank).GetBuffer());

	strUTM = strUTM.Mid(iBlank + 1, strUTM.GetLength() - iBlank - 1);
	int jBlank = strUTM.Find(_T(' '));
	wchar_t latzone = *(strUTM.Mid(0, jBlank).GetBuffer());
	wchar_t hemisphere = getHemisphere(latzone);

	strUTM = strUTM.Mid(jBlank + 1, strUTM.GetLength() - jBlank - 1);
	int iX = strUTM.Find(_T(' '));
	easting = _tstof(strUTM.Mid(0, iX).GetBuffer());

	strUTM = strUTM.Mid(iX + 1, strUTM.GetLength() - iX - 1);
	northing = _tstof(strUTM.GetBuffer());

	if (hemisphere == _T('S'))
	{
		northing = 10000000 - northing;
	}

	setVariables();

	*latitude = 180 * (phi1 - fact1 * (fact2 + fact3 + fact4)) / PI;

	if (zone > 0)
	{
		zoneCM = 6 * zone - 183.0;
	}
	else
	{
		zoneCM = 3.0;

	}

	*longitude = zoneCM - _a3;
	if (hemisphere == _T('S'))
	{
		*latitude = - (*latitude);
	}
}

wchar_t UTM2LonLat::getHemisphere(wchar_t latzone)
{
	wchar_t hemisphere = _T('N');
	if ( NULL != _tcschr(southernHemisphere, latzone) )
	{
		hemisphere = _T('S');
	}
	return hemisphere;
}

void UTM2LonLat::setVariables()
{
	arc = northing / k0;
	mu = arc
		/ (a * (1 - pow(e, 2) / 4.0 - 3 * pow(e, 4) / 64.0 - 5 * pow(e, 6) / 256.0));

	ei = (1 - pow((1 - e * e), (1 / 2.0)))
		/ (1 + pow((1 - e * e), (1 / 2.0)));

	ca = 3 * ei / 2 - 27 * pow(ei, 3) / 32.0;

	cb = 21 * pow(ei, 2) / 16 - 55 * pow(ei, 4) / 32;
	cc = 151 * pow(ei, 3) / 96;
	cd = 1097 * pow(ei, 4) / 512;
	phi1 = mu + ca * sin(2 * mu) + cb * sin(4 * mu) + cc * sin(6 * mu) + cd
		* sin(8 * mu);

	n0 = a / pow((1 - pow((e * sin(phi1)), 2)), (1 / 2.0));

	r0 = a * (1 - e * e) / pow((1 - pow((e * sin(phi1)), 2)), (3 / 2.0));
	fact1 = n0 * tan(phi1) / r0;

	_a1 = 500000 - easting;
	dd0 = _a1 / (n0 * k0);
	fact2 = dd0 * dd0 / 2;

	t0 = pow(tan(phi1), 2);
	Q0 = e1sq * pow(cos(phi1), 2);
	fact3 = (5 + 3 * t0 + 10 * Q0 - 4 * Q0 * Q0 - 9 * e1sq) * pow(dd0, 4)
		/ 24;

	fact4 = (61 + 90 * t0 + 298 * Q0 + 45 * t0 * t0 - 252 * e1sq - 3 * Q0
		* Q0)
		* pow(dd0, 6) / 720;

	//
	lof1 = _a1 / (n0 * k0);
	lof2 = (1 + 2 * t0 + Q0) * pow(dd0, 3) / 6.0;
	lof3 = (5 - 2 * Q0 + 28 * t0 - 3 * pow(Q0, 2) + 8 * e1sq + 24 * pow(t0, 2))
		* pow(dd0, 5) / 120;
	_a2 = (lof1 - lof2 + lof3) / cos(phi1);
	_a3 = _a2 * 180 / PI;
}